#include <thread.h>

static const uint8_t prio_bitmap_buf[] = {
	0, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	7, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0
};

struct thread *current_thread;

static uint16_t scheduler_lock_nest;

static struct list thread_prio_tab[THREAD_MAX_PRIORITY];

static uint32_t ready_thread_prio;

/* 获取第一个置为1的位置 */
static uint8_t get_first_setbit(void)
{
	return prio_bitmap_buf[ready_thread_prio];
}

/* 获取最高优先级线程 */
static struct thread *get_heigt_prio_thread(void)
{
	uint8_t tmp = get_first_setbit();
	struct list *list;

	list = &thread_prio_tab[tmp];

	return list_entry(list->next, struct thread, list);
}

/* 调度器初始化 */
void schedule_init(void)
{
	/* 初始化优先级表 */
	for (uint8_t i = 0; i < THREAD_MAX_PRIORITY; ++i)
		init_list(&thread_prio_tab[i]);

	/* 就绪线程优先级 */
	ready_thread_prio = 0;
}

/* 启动任务调度器 */
void schedule_start(void)
{
	struct thread *thread_to;

	scheduler_lock_nest = 0;

	thread_to = get_heigt_prio_thread();

	thread_to->status |= THREAD_RUNNING;

	current_thread = thread_to;

	context_switch_to((ubase_t)&thread_to->sp);
	/* 将不会执行到这里 */
}

/* 从调度器中移除一个线程(移除就绪队列) */
void schedule_remove(struct thread *thread)
{
	register base_t level;

	level = interrupt_disable();

	list_del(&thread->list);
	/* 如果删除这个线程后，优先级列表已经为空则将该优先级标志位置0 */
	if (list_empty(&thread_prio_tab[thread->priority]))
		ready_thread_prio &= ~thread->mask_priority;

	/* 将线程状态切换为挂起状态 */
	thread->status = THREAD_SUSPEND;

	interrupt_enable(level);
}

/* 将一个线程加入到调度器中(加入就绪队列) */
void schedule_add(struct thread *thread)
{
	register base_t level;

	level = interrupt_disable();
	
	if (thread == current_thread)
		goto __exit;

	/* 将线程状态切换为就绪状态 */
	thread->status = THREAD_READY;

	ready_thread_prio |= thread->mask_priority;
	/* 对于优先级相同的线程，将被放到对应优先级的优先级列表的最后 */
	list_add_tail(&thread->list, &thread_prio_tab[thread->priority]);
__exit:
	interrupt_enable(level);
}

/* 立即调度下一个线程 */
void schedule(void)
{
	base_t level;
	struct thread *thread_to;
	struct thread *thread_from;

	level = interrupt_disable();

	/* 判断是否存在调度器锁 */
	if (scheduler_lock_nest == 0) {
		/* 获得最高优先级线程 */
		thread_to = get_heigt_prio_thread();

		/* 如果需要切换的线程是当前线程则不需要切换 */
		if (thread_to != current_thread) {
			thread_from = current_thread;
			current_thread = thread_to;

			thread_from->status &= ~THREAD_RUNNING;
			context_switch((ubase_t)&thread_from->sp, (ubase_t)&thread_to->sp);

			/* 将线程状态切换为运行状态 */
			thread_to->status |= THREAD_RUNNING;
		}

		interrupt_enable(level);
	} else {
		interrupt_enable(level);
	}
}

/* 放弃CPU使用权 */
void yield(void)
{
	register ubase_t level;
	struct thread *thread;
	
	level = interrupt_disable();
	
	thread = current_thread;
	thread->remaining_tick = thread->init_tick;

	if (thread->status & THREAD_READY) {
		list_del(&thread->list);
		
		list_add_tail(&thread->list, &thread_prio_tab[thread->priority]);
		
		interrupt_enable(level);
		schedule();
	} else {
		interrupt_enable(level);
	}
}

/* 锁定任务调度器 */
void enter_critical(void)
{
	register base_t level;

	level = interrupt_disable();

	scheduler_lock_nest++;

	interrupt_enable(level);
}

/* 解锁任务调度器 */
void exit_critical(void)
{
	register base_t level;

	level = interrupt_disable();

	scheduler_lock_nest--;
	if (scheduler_lock_nest <= 0) {
		scheduler_lock_nest = 0;

		interrupt_enable(level);

		if (current_thread)
			schedule();
	} else {
		interrupt_enable(level);
	}
}

